#include "motion.h"

// 定义 EN 引脚
const int EN_PINS[6] = {4, 7, 17, 3, 10, 13};

// === 电机定义（对象数组，直接用 . 访问） ===
AccelStepper motors[] = 
{
    AccelStepper(AccelStepper::DRIVER, 5, 6),    // 左髋
    AccelStepper(AccelStepper::DRIVER, 15, 16),    // 左膝
    AccelStepper(AccelStepper::DRIVER, 18, 8),    // 左踝
    AccelStepper(AccelStepper::DRIVER, 46, 9),    // 右髋
    AccelStepper(AccelStepper::DRIVER, 11, 12),  // 右膝
    AccelStepper(AccelStepper::DRIVER, 14, 1)   // 右踝
};


// === 电机参数 ===
static const int STEPS_PER_REV = 200;   // 42步进电机 200步/圈
static const int MICROSTEPS    = 16;    // 16细分
static const float DEG_PER_STEP = 360.0 / (STEPS_PER_REV * MICROSTEPS); // 0.1125度/步

// === 公共函数实现 ===
void motion_init() {
    for (int i = 0; i < 6; i++) {
        pinMode(EN_PINS[i], OUTPUT);
        digitalWrite(EN_PINS[i], HIGH); // 初始禁用
    }

    for (int i = 0; i < 6; i++) {
        motors[i].setMaxSpeed(1000);
        motors[i].setAcceleration(500);
        motors[i].setSpeed(800);
    }
}

void motion_enable(bool enable) {
    for (int i = 0; i < 6; i++) {
        digitalWrite(EN_PINS[i], enable ? LOW : HIGH);
    }
}

void motion_run_test() {
    for (int i = 0; i < 6; i++) {
        motors[i].runSpeed();
    }
}
// === 工具函数 ===
float stepsToAngle(long steps) {
    return steps * DEG_PER_STEP;
}

long angleToSteps(float angle) {
    return (long)(angle / DEG_PER_STEP);
}

